#include "FastOps.h"
#include "Vector.h"
#include "List.h"

namespace RSIM{

namespace FastOps{

void spatial_basis_shift_mat(const double *C, const double *A, double *p){
	double t[146];
	
	t[2]=A[0]*C[0];
	t[3]=A[12]*C[2];
	t[4]=A[6]*C[1];
	t[5]=t[2]+t[3]+t[4];
	t[6]=A[1]*C[0];
	t[7]=A[13]*C[2];
	t[8]=A[7]*C[1];
	t[9]=t[6]+t[7]+t[8];
	t[10]=A[2]*C[0];
	t[11]=A[14]*C[2];
	t[12]=A[8]*C[1];
	t[13]=t[10]+t[11]+t[12];
	t[14]=A[10]*C[1];
	t[15]=A[16]*C[2];
	t[16]=A[4]*C[0];
	t[17]=t[14]+t[15]+t[16];
	t[18]=A[11]*C[1];
	t[19]=A[17]*C[2];
	t[20]=A[5]*C[0];
	t[21]=t[18]+t[19]+t[20];
	t[22]=A[3]*C[0];
	t[23]=A[15]*C[2];
	t[24]=A[9]*C[1];
	t[25]=t[22]+t[23]+t[24];
	t[26]=A[0]*C[3];
	t[27]=A[12]*C[5];
	t[28]=A[6]*C[4];
	t[29]=t[26]+t[27]+t[28];
	t[30]=A[1]*C[3];
	t[31]=A[13]*C[5];
	t[32]=A[7]*C[4];
	t[33]=t[30]+t[31]+t[32];
	t[34]=A[2]*C[3];
	t[35]=A[14]*C[5];
	t[36]=A[8]*C[4];
	t[37]=t[34]+t[35]+t[36];
	t[38]=A[10]*C[4];
	t[39]=A[16]*C[5];
	t[40]=A[4]*C[3];
	t[41]=t[38]+t[39]+t[40];
	t[42]=A[11]*C[4];
	t[43]=A[17]*C[5];
	t[44]=A[5]*C[3];
	t[45]=t[42]+t[43]+t[44];
	t[46]=A[3]*C[3];
	t[47]=A[15]*C[5];
	t[48]=A[9]*C[4];
	t[49]=t[46]+t[47]+t[48];
	t[50]=A[0]*C[6];
	t[51]=A[12]*C[8];
	t[52]=A[6]*C[7];
	t[53]=t[50]+t[51]+t[52];
	t[54]=A[1]*C[6];
	t[55]=A[13]*C[8];
	t[56]=A[7]*C[7];
	t[57]=t[54]+t[55]+t[56];
	t[58]=A[2]*C[6];
	t[59]=A[14]*C[8];
	t[60]=A[8]*C[7];
	t[61]=t[58]+t[59]+t[60];
	t[62]=A[10]*C[7];
	t[63]=A[16]*C[8];
	t[64]=A[4]*C[6];
	t[65]=t[62]+t[63]+t[64];
	t[66]=A[11]*C[7];
	t[67]=A[17]*C[8];
	t[68]=A[5]*C[6];
	t[69]=t[66]+t[67]+t[68];
	t[70]=A[3]*C[6];
	t[71]=A[15]*C[8];
	t[72]=A[9]*C[7];
	t[73]=t[70]+t[71]+t[72];
	t[74]=A[18]*C[0];
	t[75]=A[24]*C[1];
	t[76]=A[30]*C[2];
	t[77]=t[74]+t[75]+t[76];
	t[78]=A[19]*C[0];
	t[79]=A[25]*C[1];
	t[80]=A[31]*C[2];
	t[81]=t[78]+t[79]+t[80];
	t[82]=A[20]*C[0];
	t[83]=A[26]*C[1];
	t[84]=A[32]*C[2];
	t[85]=t[82]+t[83]+t[84];
	t[86]=A[21]*C[0];
	t[87]=A[27]*C[1];
	t[88]=A[33]*C[2];
	t[89]=t[86]+t[87]+t[88];
	t[90]=A[22]*C[0];
	t[91]=A[28]*C[1];
	t[92]=A[34]*C[2];
	t[93]=t[90]+t[91]+t[92];
	t[94]=A[23]*C[0];
	t[95]=A[29]*C[1];
	t[96]=A[35]*C[2];
	t[97]=t[94]+t[95]+t[96];
	t[98]=A[18]*C[3];
	t[99]=A[24]*C[4];
	t[100]=A[30]*C[5];
	t[101]=t[100]+t[98]+t[99];
	t[102]=A[19]*C[3];
	t[103]=A[25]*C[4];
	t[104]=A[31]*C[5];
	t[105]=t[102]+t[103]+t[104];
	t[106]=A[20]*C[3];
	t[107]=A[26]*C[4];
	t[108]=A[32]*C[5];
	t[109]=t[106]+t[107]+t[108];
	t[110]=A[21]*C[3];
	t[111]=A[27]*C[4];
	t[112]=A[33]*C[5];
	t[113]=t[110]+t[111]+t[112];
	t[114]=A[22]*C[3];
	t[115]=A[28]*C[4];
	t[116]=A[34]*C[5];
	t[117]=t[114]+t[115]+t[116];
	t[118]=A[23]*C[3];
	t[119]=A[29]*C[4];
	t[120]=A[35]*C[5];
	t[121]=t[118]+t[119]+t[120];
	t[122]=A[18]*C[6];
	t[123]=A[24]*C[7];
	t[124]=A[30]*C[8];
	t[125]=t[122]+t[123]+t[124];
	t[126]=A[19]*C[6];
	t[127]=A[25]*C[7];
	t[128]=A[31]*C[8];
	t[129]=t[126]+t[127]+t[128];
	t[130]=A[20]*C[6];
	t[131]=A[26]*C[7];
	t[132]=A[32]*C[8];
	t[133]=t[130]+t[131]+t[132];
	t[134]=A[21]*C[6];
	t[135]=A[27]*C[7];
	t[136]=A[33]*C[8];
	t[137]=t[134]+t[135]+t[136];
	t[138]=A[22]*C[6];
	t[139]=A[28]*C[7];
	t[140]=A[34]*C[8];
	t[141]=t[138]+t[139]+t[140];
	t[142]=A[23]*C[6];
	t[143]=A[29]*C[7];
	t[144]=A[35]*C[8];
	t[145]=t[142]+t[143]+t[144];
	
	p[0]=C[2]*t[13]+C[0]*t[5]+C[1]*t[9];
	p[1]=C[5]*t[13]+C[3]*t[5]+C[4]*t[9];
	p[2]=C[8]*t[13]+C[6]*t[5]+C[7]*t[9];
	p[3]=C[0]*t[25]+C[1]*t[17]+C[2]*t[21];
	p[4]=C[3]*t[25]+C[4]*t[17]+C[5]*t[21];
	p[5]=C[6]*t[25]+C[7]*t[17]+C[8]*t[21];
	p[6]=C[0]*t[29]+C[1]*t[33]+C[2]*t[37];
	p[7]=C[3]*t[29]+C[4]*t[33]+C[5]*t[37];
	p[8]=C[6]*t[29]+C[7]*t[33]+C[8]*t[37];
	p[9]=C[0]*t[49]+C[1]*t[41]+C[2]*t[45];
	p[10]=C[3]*t[49]+C[4]*t[41]+C[5]*t[45];
	p[11]=C[6]*t[49]+C[7]*t[41]+C[8]*t[45];
	p[12]=C[0]*t[53]+C[1]*t[57]+C[2]*t[61];
	p[13]=C[3]*t[53]+C[4]*t[57]+C[5]*t[61];
	p[14]=C[6]*t[53]+C[7]*t[57]+C[8]*t[61];
	p[15]=C[0]*t[73]+C[1]*t[65]+C[2]*t[69];
	p[16]=C[3]*t[73]+C[4]*t[65]+C[5]*t[69];
	p[17]=C[6]*t[73]+C[7]*t[65]+C[8]*t[69];
	p[18]=C[0]*t[77]+C[1]*t[81]+C[2]*t[85];
	p[19]=C[3]*t[77]+C[4]*t[81]+C[5]*t[85];
	p[20]=C[6]*t[77]+C[7]*t[81]+C[8]*t[85];
	p[21]=C[0]*t[89]+C[1]*t[93]+C[2]*t[97];
	p[22]=C[3]*t[89]+C[4]*t[93]+C[5]*t[97];
	p[23]=C[6]*t[89]+C[7]*t[93]+C[8]*t[97];
	p[24]=C[0]*t[101]+C[1]*t[105]+C[2]*t[109];
	p[25]=C[3]*t[101]+C[4]*t[105]+C[5]*t[109];
	p[26]=C[6]*t[101]+C[7]*t[105]+C[8]*t[109];
	p[27]=C[0]*t[113]+C[1]*t[117]+C[2]*t[121];
	p[28]=C[3]*t[113]+C[4]*t[117]+C[5]*t[121];
	p[29]=C[6]*t[113]+C[7]*t[117]+C[8]*t[121];
	p[30]=C[0]*t[125]+C[1]*t[129]+C[2]*t[133];
	p[31]=C[3]*t[125]+C[4]*t[129]+C[5]*t[133];
	p[32]=C[6]*t[125]+C[7]*t[129]+C[8]*t[133];
	p[33]=C[0]*t[137]+C[1]*t[141]+C[2]*t[145];
	p[34]=C[3]*t[137]+C[4]*t[141]+C[5]*t[145];
	p[35]=C[6]*t[137]+C[7]*t[141]+C[8]*t[145];
}

///////////////////////////////////////////////////////

void basis_shift_mat33(const double *C, const double *A, double *p){
	double t[38];
	t[2]=A[0]*C[0];
	t[3]=A[3]*C[1];
	t[4]=A[6]*C[2];
	t[5]=t[2]+t[3]+t[4];
	t[6]=A[1]*C[0];
	t[7]=A[4]*C[1];
	t[8]=A[7]*C[2];
	t[9]=t[6]+t[7]+t[8];
	t[10]=A[2]*C[0];
	t[11]=A[5]*C[1];
	t[12]=A[8]*C[2];
	t[13]=t[10]+t[11]+t[12];
	t[14]=A[0]*C[3];
	t[15]=A[3]*C[4];
	t[16]=A[6]*C[5];
	t[17]=t[14]+t[15]+t[16];
	t[18]=A[1]*C[3];
	t[19]=A[4]*C[4];
	t[20]=A[7]*C[5];
	t[21]=t[18]+t[19]+t[20];
	t[22]=A[2]*C[3];
	t[23]=A[5]*C[4];
	t[24]=A[8]*C[5];
	t[25]=t[22]+t[23]+t[24];
	t[26]=A[0]*C[6];
	t[27]=A[3]*C[7];
	t[28]=A[6]*C[8];
	t[29]=t[26]+t[27]+t[28];
	t[30]=A[1]*C[6];
	t[31]=A[4]*C[7];
	t[32]=A[7]*C[8];
	t[33]=t[30]+t[31]+t[32];
	t[34]=A[2]*C[6];
	t[35]=A[5]*C[7];
	t[36]=A[8]*C[8];
	t[37]=t[34]+t[35]+t[36];
	p[0]=C[2]*t[13]+C[0]*t[5]+C[1]*t[9];
	p[1]=C[5]*t[13]+C[3]*t[5]+C[4]*t[9];
	p[2]=C[8]*t[13]+C[6]*t[5]+C[7]*t[9];
	p[3]=C[0]*t[17]+C[1]*t[21]+C[2]*t[25];
	p[4]=C[3]*t[17]+C[4]*t[21]+C[5]*t[25];
	p[5]=C[6]*t[17]+C[7]*t[21]+C[8]*t[25];
	p[6]=C[0]*t[29]+C[1]*t[33]+C[2]*t[37];
	p[7]=C[3]*t[29]+C[4]*t[33]+C[5]*t[37];
	p[8]=C[6]*t[29]+C[7]*t[33]+C[8]*t[37];
}

///////////////////////////////////////////////////////

void transpose_basis_shift_mat33(const double *C, const double *A, double *p){
	double t[38];
	t[2]=A[0]*C[0];
	t[3]=A[3]*C[3];
	t[4]=A[6]*C[6];
	t[5]=t[2]+t[3]+t[4];
	t[6]=A[1]*C[0];
	t[7]=A[4]*C[3];
	t[8]=A[7]*C[6];
	t[9]=t[6]+t[7]+t[8];
	t[10]=A[2]*C[0];
	t[11]=A[5]*C[3];
	t[12]=A[8]*C[6];
	t[13]=t[10]+t[11]+t[12];
	t[14]=A[0]*C[1];
	t[15]=A[3]*C[4];
	t[16]=A[6]*C[7];
	t[17]=t[14]+t[15]+t[16];
	t[18]=A[1]*C[1];
	t[19]=A[4]*C[4];
	t[20]=A[7]*C[7];
	t[21]=t[18]+t[19]+t[20];
	t[22]=A[2]*C[1];
	t[23]=A[5]*C[4];
	t[24]=A[8]*C[7];
	t[25]=t[22]+t[23]+t[24];
	t[26]=A[0]*C[2];
	t[27]=A[3]*C[5];
	t[28]=A[6]*C[8];
	t[29]=t[26]+t[27]+t[28];
	t[30]=A[1]*C[2];
	t[31]=A[4]*C[5];
	t[32]=A[7]*C[8];
	t[33]=t[30]+t[31]+t[32];
	t[34]=A[2]*C[2];
	t[35]=A[5]*C[5];
	t[36]=A[8]*C[8];
	t[37]=t[34]+t[35]+t[36];
	p[0]=C[0]*t[5]+C[6]*t[13]+C[3]*t[9];
	p[1]=C[1]*t[5]+C[7]*t[13]+C[4]*t[9];
	p[2]=C[2]*t[5]+C[8]*t[13]+C[5]*t[9];
	p[3]=C[0]*t[17]+C[3]*t[21]+C[6]*t[25];
	p[4]=C[1]*t[17]+C[4]*t[21]+C[7]*t[25];
	p[5]=C[2]*t[17]+C[5]*t[21]+C[8]*t[25];
	p[6]=C[0]*t[29]+C[3]*t[33]+C[6]*t[37];
	p[7]=C[1]*t[29]+C[4]*t[33]+C[7]*t[37];
	p[8]=C[2]*t[29]+C[5]*t[33]+C[8]*t[37];
}

///////////////////////////////////////////////////////

void inv_mat3x3(const double *c, double *p){
	double det = c[0]*c[4]*c[8] - c[0]*c[5]*c[7] - c[1]*c[3]*c[8] + c[1]*c[5]*c[6] + c[2]*c[3]*c[7] - c[2]*c[4]*c[6];
	if(fabs(det)>1.0E-10){
		p[0]=(c[4]*c[8] - c[5]*c[7])/det;
		p[1]=(c[2]*c[7] - c[1]*c[8])/det;
		p[2]=(c[1]*c[5] - c[2]*c[4])/det;
		p[3]=(c[5]*c[6] - c[3]*c[8])/det;
		p[4]=(c[0]*c[8] - c[2]*c[6])/det;
		p[5]=(c[2]*c[3] - c[0]*c[5])/det;
		p[6]=(c[3]*c[7] - c[4]*c[6])/det;
		p[7]=(c[1]*c[6] - c[0]*c[7])/det;
		p[8]=(c[0]*c[4] - c[1]*c[3])/det;
	}
	else{
		cout<<"\ninv_mat3x3(): Matrix is poorly conditioned\n";
		exit(0);
	}
}

///////////////////////////////////////////////////////

void spatial_basis_shift_vec(const double *c, const double *m, double *p){
	p[0]=c[0]*m[0] + c[1]*m[1] + c[2]*m[2];
	p[1]=c[3]*m[0] + c[4]*m[1] + c[5]*m[2];
	p[2]=c[6]*m[0] + c[7]*m[1] + c[8]*m[2];
	p[3]=c[0]*m[3] + c[1]*m[4] + c[2]*m[5];
	p[4]=c[3]*m[3] + c[4]*m[4] + c[5]*m[5];
	p[5]=c[6]*m[3] + c[7]*m[4] + c[8]*m[5];
}

///////////////////////////////////////////////////////

void transpose_mult_mat33_vec3(const double *m, const double *n, double *p){	
	p[0]=m[0]*n[0] + m[3]*n[1] + m[6]*n[2];
	p[1]=m[1]*n[0] + m[4]*n[1] + m[7]*n[2];
	p[2]=m[2]*n[0] + m[5]*n[1] + m[8]*n[2];
}

///////////////////////////////////////////////////////

void wxwxr(const double *w, const double *r, double *wwr){
	wwr[0] = - w[1]*(r[0]*w[1] - r[1]*w[0]) - w[2]*(r[0]*w[2] - r[2]*w[0]);
	wwr[1] = w[0]*(r[0]*w[1] - r[1]*w[0]) - w[2]*(r[1]*w[2] - r[2]*w[1]);
	wwr[2] = w[0]*(r[0]*w[2] - r[2]*w[0]) + w[1]*(r[1]*w[2] - r[2]*w[1]);
}

///////////////////////////////////////////////////////

void cross(const double *a, const double *b, double *c){
	c[0] = a[1]*b[2] - a[2]*b[1];
	c[1] = a[2]*b[0] - a[0]*b[2];
	c[2] = a[0]*b[1] - a[1]*b[0];
}

///////////////////////////////////////////////////////

void mult_mat33_mat33(const double *m1, const double *m2, double *m3){
	m3[0] = m1[0]*m2[0] + m1[1]*m2[3] + m1[2]*m2[6];
	m3[1] = m1[0]*m2[1] + m1[1]*m2[4] + m1[2]*m2[7];
	m3[2] = m1[0]*m2[2] + m1[1]*m2[5] + m1[2]*m2[8];
	
	m3[3] = m1[3]*m2[0] + m1[4]*m2[3] + m1[5]*m2[6];
	m3[4] = m1[3]*m2[1] + m1[4]*m2[4] + m1[5]*m2[7];
	m3[5] = m1[3]*m2[2] + m1[4]*m2[5] + m1[5]*m2[8];
	
	m3[6] = m1[6]*m2[0] + m1[7]*m2[3] + m1[8]*m2[6];
	m3[7] = m1[6]*m2[1] + m1[7]*m2[4] + m1[8]*m2[7];
	m3[8] = m1[6]*m2[2] + m1[7]*m2[5] + m1[8]*m2[8];
}

///////////////////////////////////////////////////////

void formCrossProdMat(const double *v, double *m){
	m[0] = 0.0;	m[1] = -v[2]; 	m[2] = v[1];
	m[3] = v[2];	m[4] = 0.0;	m[5] = -v[0];
	m[6] = -v[1];	m[7] = v[0];    m[8] = 0.0;
	
}

///////////////////////////////////////////////////////

void mult_mat33_vec3(const double *m, const double *v, double *op){
	op[0] = m[0]*v[0] + m[1]*v[1] + m[2]*v[2];
	op[1] = m[3]*v[0] + m[4]*v[1] + m[5]*v[2];
	op[2] = m[6]*v[0] + m[7]*v[1] + m[8]*v[2];
}

///////////////////////////////////////////////////////

void transpose_mult_mat33_vec3(const double *m, const double& n0,const double& n1,const double& n2, double *p){
	p[0]=m[0]*n0 + m[3]*n1 + m[6]*n2;
	p[1]=m[1]*n0 + m[4]*n1 + m[7]*n2;
	p[2]=m[2]*n0 + m[5]*n1 + m[8]*n2;
}

///////////////////////////////////////////////////////

void mult_mat66_vec6(const double *I, const double *F, double *p){
	p[0]=F[0]*I[0]+F[1]*I[1]+F[2]*I[2]+F[3]*I[3]+F[4]*I[4]+F[5]*I[5];
	p[1]=F[4]*I[10]+F[0]*I[6]+F[5]*I[11]+F[1]*I[7]+F[2]*I[8]+F[3]*I[9];
	p[2]=F[0]*I[12]+F[1]*I[13]+F[2]*I[14]+F[3]*I[15]+F[4]*I[16]+F[5]*I[17];
	p[3]=F[0]*I[18]+F[1]*I[19]+F[2]*I[20]+F[3]*I[21]+F[4]*I[22]+F[5]*I[23];
	p[4]=F[0]*I[24]+F[1]*I[25]+F[2]*I[26]+F[3]*I[27]+F[4]*I[28]+F[5]*I[29];
	p[5]=F[0]*I[30]+F[1]*I[31]+F[2]*I[32]+F[3]*I[33]+F[4]*I[34]+F[5]*I[35];
}

///////////////////////////////////////////////////////

void transpose_spatial_basis_shift_vec(const double *C, const double *A, double *p){
	p[0]=A[0]*C[0]+A[1]*C[3]+A[2]*C[6];
	p[1]=A[0]*C[1]+A[1]*C[4]+A[2]*C[7];
	p[2]=A[0]*C[2]+A[1]*C[5]+A[2]*C[8];
	p[3]=A[3]*C[0]+A[4]*C[3]+A[5]*C[6];
	p[4]=A[3]*C[1]+A[4]*C[4]+A[5]*C[7];
	p[5]=A[3]*C[2]+A[4]*C[5]+A[5]*C[8];
}

///////////////////////////////////////////////////////

void transpose_basis_shift_vec3(const double *C, const double *A, double *p){
	p[0]=A[0]*C[0] + A[1]*C[3] + A[2]*C[6];
	p[1]=A[0]*C[1] + A[1]*C[4] + A[2]*C[7];
	p[2]=A[0]*C[2] + A[1]*C[5] + A[2]*C[8];
}

///////////////////////////////////////////////////////

void shift_spatial_vec(const double *r, const double *v, double *p){
	
	p[0] = v[0] + r[1]*v[5] - r[2]*v[4];
	p[1] = v[1] - r[0]*v[5] + r[2]*v[3];
	p[2] = v[2] + r[0]*v[4] - r[1]*v[3];
        p[3] = v[3];
	p[4] = v[4];
        p[5] = v[5];
}

///////////////////////////////////////////////////////

void spatial_shift_vec6(const double *F, const double *beta, double *p){ 
	p[0]=F[0]-F[4]*beta[2]+F[5]*beta[1];
	p[1]=F[1]+F[3]*beta[2]-F[5]*beta[0];
	p[2]=F[2]-F[3]*beta[1]+F[4]*beta[0];
        p[3]=F[3];
        p[4]=F[4];
        p[5]=F[5];
}

///////////////////////////////////////////////////////

void spatial_shift_vec3(const double *F, const double *beta, double *p){
	p[0]=F[2]*beta[1]-F[1]*beta[2];
	p[1]=F[0]*beta[2]-F[2]*beta[0];
	p[2]=F[1]*beta[0]-F[0]*beta[1];
        p[3]=F[0];
        p[4]=F[1];
	p[5]=F[2];
}

///////////////////////////////////////////////////////

void transpose_skbeta_x_V(const double *A, const double *beta, double *p){
	p[0]=A[0];
	p[1]=A[1];
	p[2]=A[2];
	p[3]=A[3]+A[1]*beta[2]-A[2]*beta[1];
	p[4]=A[4]-A[0]*beta[2]+A[2]*beta[0];
	p[5]=A[5]+A[0]*beta[1]-A[1]*beta[0];
}

///////////////////////////////////////////////////////

} // namespace FastOps

}// namespace RSIM